/* USER CODE BEGIN Header */
/*
 * 设备操作流程：
 * 1、设备上电，默认读取电机状态首先检查是否为闭环模式，向上报告
 * 2、确保所有电机为闭环模式，向上报告
 * 3、检查电机是否处于对应的控制模式，向上报告
 * 4、确保所有电机处于对应的控制模式，向上报告
 * 5、向上报告初始化成功
 * 6、每个固定时间检查电机的位置和速度等相关信息，定时向上报告
 * 7、接收到上位机信息，分包处理数据
 * 注：串口1为上位机通讯接口
 */

/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "string.h"
#include <stdbool.h> // 用于bool类型
#include <stdio.h>   // 用于sscanf函数
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart2_rx;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
uint8_t message[50] = {}; // 上位机接收缓冲区
uint8_t dev1_rx_buf[50];  // 设备1接收缓冲区 (USART2)
uint8_t dev2_rx_buf[50];  // 设备2接收缓冲区 (USART3)
uint8_t dev3_rx_buf[50];  // 设备3接收缓冲区 (USART6)

// 设备地址定义
#define DEVICE_1_ADDR 0x01
#define DEVICE_2_ADDR 0x02
#define DEVICE_3_ADDR 0x03

// 控制模式定义
#define MODE_TORQUE    0
#define MODE_SPEED     1
#define MODE_POSITION  2

// 设备状态
typedef struct {
    uint8_t address;
    bool closedLoop;
    uint8_t currentMode;
} DeviceState;


//默认状态下，设备1 3 位置模式闭环  设备2 速度模式闭环
DeviceState deviceStates[] = {
    {DEVICE_1_ADDR, false, MODE_TORQUE},
    {DEVICE_2_ADDR, false, MODE_TORQUE},
    {DEVICE_3_ADDR, false, MODE_TORQUE}
};


// 寄存器地址
#define REG_CLOSED_LOOP 0x00A2
#define REG_CONTROL_MODE 0x0060
#define REG_SPEED_SETPOINT 0x0021
#define REG_POSITION_SETPOINT 0x0023

//初始化流程标志位
uint8_t initFlag=0;
uint8_t deviceCount=3;
uint8_t device_mode[]={MODE_POSITION,MODE_SPEED,MODE_POSITION};
uint32_t Device_checkTime=1000;

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
uint16_t calculateModbusCRC(uint8_t *data, uint16_t length);	//校验码生成
bool crc16_Check(uint8_t *data, uint16_t length);				//校验码比较

bool setClosedLoop(uint8_t deviceAddress);						//设置FOC闭环模式
void FOC_Mode(uint8_t deviceAddress, uint8_t mode);				//选择FOC控制模式

void FOC_Speed(uint8_t deviceAddress, double rpm);				//设置FOC速度
void FOC_Angle(uint8_t deviceAddress, double degrees);			//设置FOC的角度

double FOC_Speed_deal(uint8_t *response, uint16_t length);		//读取FOC的速度
double FOC_Angle_deal(uint8_t *response, uint16_t length);		//读取FOC的角度


bool DeviceStateCheck();
bool DeviceStateDeal();
bool DeviceModeCheck();
bool DeviceModeDeal();

void SetInit();
bool initializeDevices(void);									//设备初始化信息，发送电机初始数据


/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_USART3_UART_Init();
  MX_USART6_UART_Init();
  /* USER CODE BEGIN 2 */
  // 初始化上位机接收
  HAL_UARTEx_ReceiveToIdle_DMA(&huart1, message, sizeof(message));
  __HAL_DMA_DISABLE_IT(&hdma_usart1_rx, DMA_IT_HT);

  // 初始化设备串口接收
  HAL_UARTEx_ReceiveToIdle_DMA(&huart2, dev1_rx_buf, sizeof(dev1_rx_buf));
  HAL_UARTEx_ReceiveToIdle_DMA(&huart3, dev2_rx_buf, sizeof(dev2_rx_buf));
  HAL_UARTEx_ReceiveToIdle_DMA(&huart6, dev3_rx_buf, sizeof(dev3_rx_buf));

  //初始化设备信息
  SetInit();
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_2);
    HAL_Delay(1000);
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 8;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
/*****************************************************************************************************************/
/*
 * 串口数据处理部分
 */
/*****************************************************************************************************************/

//不定长串口中断回调函数
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    // 根据上位机进行数据转发
    if (huart == &huart1)
    {

    }

    //设备1接收数据处理
    if(huart == &huart2)
    {

    }

    //设备2接收数据处理
    if(huart == &huart3)
    {

    }

    //设备3接收数据处理
    if(huart == &huart6)
    {

    }
}

/*****************************************************************************************************************/
/*
 * 基础类函数，用于完成最基础的工作
 */
/*****************************************************************************************************************/

// CRC16计算函数（Modbus RTU标准）
uint16_t calculateModbusCRC(uint8_t *data, uint16_t length) {
    uint16_t crc = 0xFFFF;  // 初始值
    for (uint16_t i = 0; i < length; ++i) {
        crc ^= (uint16_t)data[i];  // 异或当前字节
        for (uint8_t j = 0; j < 8; ++j) {
            uint8_t lsb = crc & 0x0001;  // 检查最低位
            crc >>= 1;               // 右移1位
            if (lsb) crc ^= 0xA001;  // 如果最低位为1，异或多项式
        }
    }
    return crc;
}

// CRC16校验函数
bool crc16_Check(uint8_t *data, uint16_t length) {
    if (length < 3) return false; // 至少需要地址、功能码和CRC

    // 提取数据部分（除去最后两个CRC字节）
    uint16_t dataLength = length - 2;

    // 计算CRC
    uint16_t calculatedCRC = calculateModbusCRC(data, dataLength);

    // 获取帧中的CRC（小端序）
    uint16_t receivedCRC = (data[length-1] << 8) | data[length-2];

    return (calculatedCRC == receivedCRC);
}

// 设置闭环模式
bool setClosedLoop(uint8_t deviceAddress) {
    uint8_t frame[8];

    // 构造帧
    frame[0] = deviceAddress;      // 设备地址
    frame[1] = 0x06;               // 功能码(写单个寄存器)
    frame[2] = (REG_CLOSED_LOOP >> 8) & 0xFF; // 寄存器地址高字节
    frame[3] = REG_CLOSED_LOOP & 0xFF;        // 寄存器地址低字节
    frame[4] = 0x00;               	// 数据高字节
    frame[5] = 0x01;				// 数据低字节

    // 计算CRC
    uint16_t crc = calculateModbusCRC(frame, 6);
    frame[6] = crc & 0xFF;         // CRC低字节
    frame[7] = crc >> 8;           // CRC高字节


    // 根据设备地址发送到对应串口
    if(deviceAddress == DEVICE_1_ADDR) {
        HAL_UART_Transmit(&huart2, frame, 8, 20);
        HAL_Delay(Device_checkTime);
        if(deviceStates[0].closedLoop) return true;
    } else if(deviceAddress == DEVICE_2_ADDR) {
        HAL_UART_Transmit(&huart3, frame, 8, 20);
        HAL_Delay(Device_checkTime);
        if(deviceStates[1].closedLoop) return true;
    } else if(deviceAddress == DEVICE_3_ADDR) {
        HAL_UART_Transmit(&huart6, frame, 8, 20);
        HAL_Delay(Device_checkTime);
        if(deviceStates[2].closedLoop) return true;
    }

    return false;
}

// 设置驱动器控制模式
void FOC_Mode(uint8_t deviceAddress, uint8_t mode) {
    uint8_t frame[8];

    // 构造帧
    frame[0] = deviceAddress;      // 设备地址
    frame[1] = 0x06;               // 功能码(写单个寄存器)
    frame[2] = (REG_CONTROL_MODE >> 8) & 0xFF; // 寄存器地址高字节
    frame[3] = REG_CONTROL_MODE & 0xFF;        // 寄存器地址低字节
    frame[4] = 0x00;               // 数据高字节
    frame[5] = mode;               // 数据低字节(模式值)

    // 计算CRC
    uint16_t crc = calculateModbusCRC(frame, 6);
    frame[6] = crc & 0xFF;         // CRC低字节
    frame[7] = crc >> 8;           // CRC高字节

    // 根据设备地址发送到对应串口
    if(deviceAddress == DEVICE_1_ADDR) {
        HAL_UART_Transmit(&huart2, frame, 8, 20);
    } else if(deviceAddress == DEVICE_2_ADDR) {
        HAL_UART_Transmit(&huart3, frame, 8, 20);
    } else if(deviceAddress == DEVICE_3_ADDR) {
        HAL_UART_Transmit(&huart6, frame, 8, 20);
    }

}

// 设置驱动器转速
void FOC_Speed(uint8_t deviceAddress, double rpm) {
    // 转换为整数值（放大100倍）
    int32_t value = (int32_t)(rpm * 100);

    uint8_t frame[13]; // 需要13字节的缓冲区

    // 构造帧
    frame[0] = deviceAddress;      // 设备地址
    frame[1] = 0x10;               // 功能码(写多个寄存器)
    frame[2] = (REG_SPEED_SETPOINT >> 8) & 0xFF; // 起始地址高字节
    frame[3] = REG_SPEED_SETPOINT & 0xFF;        // 起始地址低字节
    frame[4] = 0x00;               // 寄存器数量高字节
    frame[5] = 0x02;               // 寄存器数量低字节(2个寄存器)
    frame[6] = 0x04;               // 字节数(4字节)

    // 添加速度值(大端序32位有符号整数)
    frame[7] = (value >> 24) & 0xFF;  // 最高字节
    frame[8] = (value >> 16) & 0xFF;
    frame[9] = (value >> 8) & 0xFF;
    frame[10] = value & 0xFF;         // 最低字节

    // 计算CRC (11字节数据)
    uint16_t crc = calculateModbusCRC(frame, 11);
    frame[11] = crc & 0xFF;         // CRC低字节
    frame[12] = crc >> 8;           // CRC高字节

    // 根据设备地址发送到对应串口
    if(deviceAddress == DEVICE_1_ADDR) {
        HAL_UART_Transmit(&huart2, frame, 13, 20);
    } else if(deviceAddress == DEVICE_2_ADDR) {
        HAL_UART_Transmit(&huart3, frame, 13, 20);
    } else if(deviceAddress == DEVICE_3_ADDR) {
        HAL_UART_Transmit(&huart6, frame, 13, 20);
    }
}

// 读取实时速度(返回单位为RPM)
double FOC_Speed_deal(uint8_t *response, uint16_t length) {
    if(length < 9 || !crc16_Check(response, length)) {
        return 0.0; // 无效响应
    }

    // 解析32位有符号整数(大端序)
    int32_t value = (response[3] << 24) |
                    (response[4] << 16) |
                    (response[5] << 8) |
                    response[6];

    // 转换为实际速度(放大100倍)
    return (double)value / 100.0;
}

// 设置驱动器角度
void FOC_Angle(uint8_t deviceAddress, double degrees) {
    // 计算放大100倍后的值
    int32_t value = (int32_t)(degrees * 100);

    uint8_t frame[13]; // 需要13字节的缓冲区

    // 构造帧
    frame[0] = deviceAddress;      // 设备地址
    frame[1] = 0x10;               // 功能码(写多个寄存器)
    frame[2] = (REG_POSITION_SETPOINT >> 8) & 0xFF; // 起始地址高字节
    frame[3] = REG_POSITION_SETPOINT & 0xFF;        // 起始地址低字节
    frame[4] = 0x00;               // 寄存器数量高字节
    frame[5] = 0x02;               // 寄存器数量低字节(2个寄存器)
    frame[6] = 0x04;               // 字节数(4字节)

    // 添加位置值(大端序)
    frame[7] = (value >> 24) & 0xFF;
    frame[8] = (value >> 16) & 0xFF;
    frame[9] = (value >> 8) & 0xFF;
    frame[10] = value & 0xFF;

    // 计算CRC (11字节数据)
    uint16_t crc = calculateModbusCRC(frame, 11);
    frame[11] = crc & 0xFF;         // CRC低字节
    frame[12] = crc >> 8;           // CRC高字节

    // 根据设备地址发送到对应串口
    if(deviceAddress == DEVICE_1_ADDR) {
        HAL_UART_Transmit(&huart2, frame, 13, 20);
    } else if(deviceAddress == DEVICE_2_ADDR) {
        HAL_UART_Transmit(&huart3, frame, 13, 20);
    } else if(deviceAddress == DEVICE_3_ADDR) {
        HAL_UART_Transmit(&huart6, frame, 13, 20);
    }
}

// 读取实时位置(返回单位为度)
double FOC_Angle_deal(uint8_t *response, uint16_t length) {
    if(length < 9 || !crc16_Check(response, length)) {
        return 0.0; // 无效响应
    }

    // 解析32位有符号整数(大端序)
    int32_t value = (response[3] << 24) |
                    (response[4] << 16) |
                    (response[5] << 8) |
                    response[6];

    // 转换为实际位置(放大100倍)
    return (double)value / 100.0;
}

//检查设备的状态
bool DeviceStateCheck()
{
	uint8_t i;
	for(i=0;i<deviceCount;i++)
	{
		if(!deviceStates[i].closedLoop) return false;
	}

	return true;
}

//检查设备的控制模式
bool DeviceModeCheck()
{
	uint8_t i;
	for(i=0;i<deviceCount;i++)
	{
		if(deviceStates[i].currentMode!=device_mode[i]) return false;
	}

	return true;
}

//处理设备的状态
bool DeviceStateDeal()
{
	uint8_t i;
	for(i=0;i<deviceCount;i++)
	{
		if(!deviceStates[i].closedLoop)
		{
			if(!setClosedLoop(deviceStates[i].address))	return false;
		}
	}
	return true;
}

//处理设备的控制模式
bool DeviceModeDeal()
{
	return true;
}

/*****************************************************************************************************************/
/*
 * 组合类函数，属于基础类函数的组合使用
 */
/*****************************************************************************************************************/

/*	检查并初始化设备：检查和设置闭环模式和控制模式
 * 1、检查是否为闭环模式且能否设置为闭环模式
 * 2、检查是否为对应的控制模式且能否设置为对应的控制模式
 */
bool initializeDevices()
{
	//第一步设备模式检查
	if(!DeviceStateCheck())
	{
		if(DeviceModeDeal())
		{
			initFlag = 1;
		}
	}else{
		initFlag = 1;
	}

	//第二步控制模式检查
	if(initFlag & DeviceModeCheck())
	{
		return true;
	}else{
		if(DeviceModeDeal())
		{
			return true;
		}
		return false;
	}
}

/*	初始化设备：不考虑任何情况逐步设置闭环模式和控制模式
 * 1、所有设备设置为闭环模式，每次间隔时间为Device_checkTime（ms）
 * 2、所有设备设置为对应的控制模式，每次间隔时间为Device_checkTime（ms）
 */

void SetInit()
{
	for(int i=0;i<deviceCount;i++)
	{
		setClosedLoop(deviceStates[i].address);
		HAL_Delay(1000);
	}

	for(int i=0;i<deviceCount;i++)
	{
		FOC_Mode(deviceStates[i].address,device_mode[i]);
		HAL_Delay(1000);
	}

}

/*****************************************************************************************************************/
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
